#include <ros/ros.h>
#include <tf/tf.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include "sensor_msgs/Joy.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include <string>
#include <vector>
#include <math.h>

enum class TwistType { NAV_CMD, HAND_STICK };

class AutoDiffBase{
public:
   AutoDiffBase();
   
private:
   ~AutoDiffBase();
   void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg);
   void joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg);
   void odomSendTimerCallback(const ros::TimerEvent &);
   void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg);
   void handStickCmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg);
   void cmdSendTimerCallback(const ros::TimerEvent &);

   double rpm2mps(double rpm){
      return rpm / 60.0 * M_PI * wheel_diameter_;
   }

   double mps2rpm(double mps){
      return mps * 60.0 / M_PI / wheel_diameter_;
   }

   void getRPY(geometry_msgs::Quaternion& q_msg, double& roll, double& pitch, double& yaw){
      tf::Quaternion q;
      tf::quaternionMsgToTF(q_msg, q);
      tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
   }

   inline double sign(double x){
    return x < 0.0 ? -1.0 : 1.0;
  }

private:
   boost::mutex mutex_, imu_mutex;
   bool is_debug_, publish_tf_;
   //
   ros::Publisher odom_pub_;
   ros::Subscriber imu_sub_, joy_sub_, cmd_sub_, hand_stick_cmd_sub_;
   ros::Timer odom_timer_, cmd_send_timer_;
   tf::TransformBroadcaster odom_broadcaster_;
   //global msg
   sensor_msgs::Imu imu_;

   //launch parameters
   std::string imu_topic_, joy_topic_, hand_stick_cmd_topic_;
   std::string odom_topic_, odom_frame_, baselink_frame_;

   double wheel_diameter_, base_width_;
   double max_x_velocity_, max_angular_velocity_;

   std::string port_;
   int baudrate_;

   int odom_rate_;

   int enable_turbo_button_;
   
   ros::Time time_odom_old_, nav_twist_time_, hs_twist_time_;
   double odom_x_, odom_y_;

   TwistType twist_type_, twist_type_old_;
   geometry_msgs::Twist nav_twist_, hs_twist_, twist_old_, twist_now_;
   bool is_imu_ok_;
};

